///////////////////////////////////////////////////////////////////////////////////////////////////
/// \file formic/genminres.h
///
/// \brief   header file for the generalized minimal residual solver
///
///////////////////////////////////////////////////////////////////////////////////////////////////

#ifndef FORMIC_GENMINRES_HEADER
#define FORMIC_GENMINRES_HEADER

#include<cmath>
#include<algorithm>
#include<complex>
#include<vector>

#include<formic/exception.h>
#include<formic/matrix_actor.h>
#include<formic/numeric/numeric.h>
#include<formic/mpi/interface.h>
#include<formic/lapack/interface.h>
#include<formic/output/output.h>
#include<formic/vector/vector.h>
#include<formic/vector/matrix.h>

//*****************************************************************
// Iterative template routine -- genminres
//
// genminres solves the unsymmetric linear system Ax = b using the 
// Generalized Minimum Residual method
//
// genminres follows the algorithm described on p. 20 of the 
// SIAM Templates book.
//
// The return value indicates convergence within max_iter (input)
// iterations (0), or no convergence within max_iter iterations (1).
//
// Upon successful return, output arguments have the following values:
//  
//        x  --  approximate solution to Ax = b
// max_iter  --  the number of iterations performed before the
//               tolerance was reached
//      tol  --  the residual after the final iteration
//  
//*****************************************************************

namespace formic {

  namespace genminres_classes {

    template<class S, class L> class VectorNorm {

      public:

        S operator()(const formic::Vector<S,L> & vec) const {
          S total = 0;
          for (int i = 0; i < vec.size(); i++) {
            const S val = std::abs(vec[i]);
            total += val * val;
          }
          return std::sqrt(total);
        }

    };

    template<class S, class L> class VectorDot {

      public:

        S operator()(const formic::Vector<S,L> & v1, const formic::Vector<S,L> & v2) const {
          if (v1.size() != v2.size())
            throw formic::Exception("vectors must be the same length to take their dot product");
          S total = 0;
          for (int i = 0; i < v1.size(); i++)
            total += v1[i] * v2[i];
          return total;
        }

    };

  }

  namespace genminres_funcs {

    template<class Real> 
    inline void GeneratePlaneRotation(Real &dx, Real &dy, Real &cs, Real &sn)
    {
      if (dy == 0.0) {
        cs = 1.0;
        sn = 0.0;
      } else if (std::abs(dy) > std::abs(dx)) {
        Real temp = dx / dy;
        sn = 1.0 / std::sqrt( 1.0 + temp*temp );
        cs = temp * sn;
      } else {
        Real temp = dy / dx;
        cs = 1.0 / std::sqrt( 1.0 + temp*temp );
        sn = temp * cs;
      }
    }

    template<class Real> 
    inline void ApplyPlaneRotation(Real &dx, Real &dy, Real &cs, Real &sn)
    {
      Real temp  =  cs * dx + sn * dy;
      dy = -sn * dx + cs * dy;
      dx = temp;
    }

    template < class Matrix, class Vector >
    inline void Update(Vector &x, int k, Matrix &h, Vector &s, const std::vector<Vector> & v) {

      Vector y(s.size());
      for (int i = 0; i < s.size(); i++)
        y[i] = s[i];

      // Backsolve:  
      for (int i = k; i >= 0; i--) {
        y(i) /= h(i,i);
        for (int j = i - 1; j >= 0; j--)
          y(j) -= h(j,i) * y(i);
      }

      for (int j = 0; j <= k; j++)
        x += v[j] * y(j);
    }

    template <class Real> inline void check_not_complex(Real x) {}

    template <class Real> inline void check_not_complex(std::complex<Real> x) {
      throw formic::Exception("genminres has not been checked to ensure it is correct for complex numbers");
    }

  } // end namespace genminres_funcs


  template < class Operator,
             class Vector,
             class Preconditioner,
             class Matrix,
             class Real,
             class NormFunc,
             class DotFunc >
  inline int genminres(Operator &A,
                       Vector &x,
                       const Vector &b,
                       const Preconditioner &M,
                       Matrix &H,
                       NormFunc &norm,
                       DotFunc &dot,
                       int &m,
                       int &max_iter,
                       Real &tol)
  {

    // not yet verified correct for complex numbers!
    formic::genminres_funcs::check_not_complex(Real());

    Real resid;
    int i, j = 1, k;
    Vector s(m+1), cs(m+1), sn(m+1), w;
    
    Real normb = norm(M.solve(b));
    Vector r = M.solve(b - A * x);
    Real beta = norm(r);
    
    if (normb == 0.0)
      normb = 1;
    
    resid = norm(r) / normb;
    if ( formic::mpi::rank() == 0 )
      formic::of << boost::format("genminres starting          residual = %.2e") % resid << std::endl;
    if (std::abs(resid) <= std::abs(tol)) {
      tol = resid;
      max_iter = 0;
      return 0;
    }

    //Vector * const v = new Vector[m+1];
    std::vector<Vector> v(m+1);

    while (j <= max_iter) {
      v[0] = r * (1.0 / beta);    // ??? r / beta
      s = 0.0;
      s(0) = beta;
      
      for (i = 0; i < m && j <= max_iter; i++, j++) {
        w = M.solve(A * v[i]);
        for (k = 0; k <= i; k++) {
          H(k, i) = dot(w, v[k]);
          w -= H(k, i) * v[k];
        }
        H(i+1, i) = norm(w);
        v[i+1] = w * (1.0 / H(i+1, i)); // ??? w / H(i+1, i)

        for (k = 0; k < i; k++)
          formic::genminres_funcs::ApplyPlaneRotation(H(k,i), H(k+1,i), cs(k), sn(k));
        
        formic::genminres_funcs::GeneratePlaneRotation(H(i,i), H(i+1,i), cs(i), sn(i));
        formic::genminres_funcs::ApplyPlaneRotation(H(i,i), H(i+1,i), cs(i), sn(i));
        formic::genminres_funcs::ApplyPlaneRotation(s(i), s(i+1), cs(i), sn(i));
        
        resid = std::abs(s(i+1)) / normb;
        if ( formic::mpi::rank() == 0 )
          formic::of << boost::format("genminres iteration %3i     residual = %.2e") % j % resid << std::endl;
        if (std::abs(resid) < std::abs(tol)) {
          formic::genminres_funcs::Update(x, i, H, s, v);
          tol = resid;
          max_iter = j;
          //delete [] v;
          return 0;
        }
      }
      formic::genminres_funcs::Update(x, i - 1, H, s, v);
      r = M.solve(b - A * x);
      beta = norm(r);
      if (std::abs(resid = beta / normb) < std::abs(tol)) {
        tol = resid;
        max_iter = j;
        //delete [] v;
        return 0;
      }
    }
    
    tol = resid;
    //delete [] v;
    return 1;

  }

} // end namespace formic

#endif
